# 
# Test the GVMsample module to register images. 
# 
# Usage: Set your Naoqi's IP address.
# 
# - Call the register function with resolution and color space. 
# - Perform a save image remote.
# - Perform a save image local
#
# Have a look at the module code provided for more details.
#

import os
import sys
import time

path = `os.environ.get("AL_DIR")`
home = `os.environ.get("HOME")`

# import NaoQi lib
if path == "None":
  print "the environnement variable AL_DIR is not set, aborting..."
  sys.exit(1)
else:
  alPath = path + "/extern/python/aldebaran"
  alPath = alPath.replace("~", home)
  alPath = alPath.replace("'", "")
  sys.path.append(alPath)
  import naoqi
  from naoqi import ALBroker
  from naoqi import ALModule
  from naoqi import ALProxy
  from naoqi import ALBehavior
  from vision_definitions import*


IP = "192.168.1.100"  # Enter your IP address here.
PORT = 9559

####
class theModule(ALModule):
  def pythondatachanged(self, param1, param2, param3):
    print "datachanged callback"


# Create proxy to GVMsample module
print "Creating GVMsample proxy"

try:
  sampleProxy = ALProxy("GVMsample", IP, PORT)
  memoryProxy = ALProxy("ALMemory", IP, PORT)
except Exception,e:
  print "Error when creating GVMSample proxy:"
  print str(e)
  exit(1)

time.sleep(1)

# Here, you can set the resolution and color space you want
# Later on, the saved images will use this format.

memoryProxy.subscribeOnData("naoVisionCurrentFrame","theModule","","pythondatachanged")

sampleProxy.unRegisterFromVIM()

print "Registering to VIM"
sampleProxy.registerToVIM(kQQVGA, kBGRColorSpace, 1)


####
# Save image in remote mode
#print "saveImageRemote"
#sampleProxy.saveImageRemote("/img", "jpg")


####
# Save image in local mode 
# (only works if GVMsample runs in the same thread as Naoqi)
#print "saveImageLocal"
#sampleProxy.saveImageLocal("/imgLocal", "jpg")
#memoryProxy.getDataOnChange

#memoryProxy.removeData("naoVisionCurrentFrame")

print "start frame grabbing"
sampleProxy.startGrabbing()

frame = memoryProxy.getData("naoVisionCurrentFrame",0)
print frame[0:20]


time.sleep(3)

print "stop frame grabbing"
sampleProxy.stopGrabbing()

frame = memoryProxy.getData("naoVisionCurrentFrame",0)
print frame[0:20]

print "unRegister"
sampleProxy.unRegisterFromVIM()


print 'end of gvm_useGVMsample python script'
frame = memoryProxy.getData("naoVisionCurrentFrame",0)
print frame[0:20]


